# @Time : 2021/4/16 11:06
# @Author : Fioman 
# @Phone : 13149920693
import numpy as np
import cv2 as cv
from scipy import linalg
import math

"""
Fioman封装的一下数学计算使用的方法,这个模块主要是来写这些数学计算的一些算子用的方法
"""


class Line(object):
    def __init__(self, x1, y1, x2, y2):
        self.x1 = x1
        self.y1 = y1
        self.x2 = x2
        self.y2 = y2

    def vector(self):
        c = (self.x1 - self.x2, self.y1 - self.y2)
        return c

    def length(self):
        length = math.sqrt(self.vector()[0] * self.vector()[0] + self.vector()[1] * self.vector()[1])
        return length


def linear_interpolation(x1, x2, y1, y2, p11Val, p12Val, p21Val, p22Val, x, y):
    """
    双线性插值
    :param x1:
    :param x2:
    :param y1:
    :param y2:
    :param p11Val: (x1,y1)的值
    :param p12Val: (x1,y2)的值
    :param p21Val: (x2,y1)的值
    :param p22Val: (x2,y2)的值
    :param x: 待求的点的坐标x
    :param y: 待求的点的坐标y
    :return: 补偿值.
    """
    interpVal = (np.dot(np.dot([x2 - x, x - x1], [[p11Val, p12Val], [p21Val, p22Val]]), [y2 - y, y - y1])) / ((x2 - x1) * (y2 - y1))
    return interpVal


def get_line_data_with_capflie(p1, p2, sections, capfileData, xInterval, yInterval, xMin, yMin, xNums, yNums, cutDeep):
    """
    获取带台面补偿的切线数据,分成了100份,然后写入到底层里面去,通过线性插值的方式
    :param p1:切线起始点1
    :param p2:切线终点2
    :param sections:分成多少段
    :param capfileData:台布测量高度表,一维列表
    :param xInterval:台布测量的x间隔
    :param yInterval:台布测量的y间隔
    :param xMin:台布测量x方向的起始坐标
    :param yMin:台布测量的y方向的起始坐标
    :param xNums:台布测量的x方向的采样个数
    :param yNums:台布测量的y方向的采样个数
    :param cutDeep:切割深度
    :return:
    """
    npTab = np.array(capfileData)
    tab = npTab.reshape((-1, 4))
    # 求出测高的均值
    zAxisAver = np.average(tab[:, 2])
    tab[:, 2] = tab[:, 2] - zAxisAver  # 补偿坐标表
    sectionPoints = []
    sectionPoints.append(p1)
    for i in range(1, sections - 1):
        x = p1[0] + (p2[0] - p1[0]) * i / (sections - 1)
        y = p1[1] + (p2[1] - p1[1]) * i / (sections - 1)
        sectionPoints.append([x, y])
    sectionPoints.append(p2)

    # 计算补偿坐标
    retData = []
    xMax = xMin + (xNums - 1) * xInterval
    yMax = yMin + (yNums - 1) * yInterval
    for p in sectionPoints:
        # 先求最近的四个点坐标
        # 先排除特殊情况,如果小于最小值,或者大于最大值
        # 先求出来点在tab里面的索引值,主要是包含几个y,还有就是第几个y,yNums是一组数据.
        m1 = int((p[0] - xMin) // xInterval)
        m2 = int((p[1] - yMin) // yInterval)
        if m1 < 0:  # p[0]小于xMin
            m1 = 0
        if m2 < 0:  # p[1]小于yMin
            m2 = 0

        # 如果超过了范围就用最近的点
        if p[0] >= xMax:
            m1 = xNums - 2
        if p[1] >= yMax:
            m2 = yNums - 2
        # 四个为矩形的参考点的坐标
        x1 = m1 * xInterval + xMin
        x2 = (m1 + 1) * xInterval + xMin
        y1 = m2 * yInterval + yMin
        y2 = (m2 + 1) * yInterval + yMin

        p11Val = tab[m1 * yNums + m2, 2]
        p12Val = tab[m1 * yNums + m2 + 1, 2]
        p21Val = tab[(m1 + 1) * yNums + m2, 2]
        p22Val = tab[(m1 + 1) * yNums + m2 + 1, 2]
        retData.extend([p[0], p[1], linear_interpolation(x1, x2, y1, y2, p11Val, p12Val, p21Val, p22Val, p[0], p[1]) + cutDeep, 0])
    return retData


def get_line_data_without_capfile(p1, p2, sections, cutDeep):
    """
    如果capfileData的数据是空的时候调用的函数
    :param p1:
    :param p2:
    :param sections:
    :param cutDeep:
    :return:
    """
    sectionPoints = []
    sectionPoints.append(p1)
    for i in range(1, sections - 1):
        x = p1[0] + (p2[0] - p1[0]) * i / (sections - 1)
        y = p1[1] + (p2[1] - p1[1]) * i / (sections - 1)
        sectionPoints.append([x, y])
    sectionPoints.append(p2)

    res = []
    # 计算补偿坐标
    for p in sectionPoints:
        res.extend([p[0], p[1], cutDeep, 0])
    return res


def distance(p1, p2):
    """
    计算平面上两个点之前的距离,精确到小数点后两位
    :param p1: 平面上的点1
    :param p2: 平面上的点2
    :return:
    """
    assert len(p1) == 2
    assert len(p2) == 2
    return round(np.sqrt((abs(p2[0] - p1[0]) ** 2 + abs(p2[1] - p1[1]) ** 2)), 2)


def calc_calibration_mtx(robotPos, imagePos):
    """
    根据三组机械手坐标和图像像素坐标计算标定矩阵
    :param robotPos: 机械手坐标
    :param imagePos: 图像坐标
    :return:
    """
    a = np.zeros((3, 3))
    matrix = np.zeros((2, 3))
    b1 = np.array((robotPos[0][0], robotPos[1][0], robotPos[2][0]))
    b2 = np.array((robotPos[0][1], robotPos[1][1], robotPos[2][1]))
    for i in range(0, 3):
        a[i] = np.array((imagePos[i][0], imagePos[i][1], 1))
    matrix[0] = linalg.solve(a, b1)
    matrix[1] = linalg.solve(a, b2)
    return matrix


def get_linear_fit_k_and_b_horizontal(pointsMat):
    """
    获取水平方向的拟合直线的k和b值,因为是水平方向,不需要考虑角度无限接近于90度的情况
    :param pointsMat:
    :return:
    """
    outPut = cv.fitLine(pointsMat, cv.DIST_L2, 0, 0.01, 0.01)
    if outPut[0] != 0:
        k = outPut[1] / outPut[0]
        k = k[0]
        b = outPut[3] - k * outPut[2]
    else:
        k = None
        b = outPut[2]
    return float(k), float(b)


def get_linear_fit_k_vertical(pointsMat):
    """
    垂直方向获取拟合直线,因为垂直方向上,有角度可能很接近90度的情况,如果大于一定的值,就认为是None
    :param pointsMat:
    :return:
    """
    outPut = cv.fitLine(pointsMat, cv.DIST_L2, 0, 0.01, 0.01)
    if outPut[0] != 0:
        k = outPut[1] / outPut[0]
        k = k[0]
        if abs(k) > 1146:
            k = None
    else:
        k = None
    return k


def get_line_k(p1, p2):
    """
    获取某一条直线的斜率
    :param p1:点1的坐标
    :param p2:点2的坐标
    :return:
    """
    if p1[0] == p2[0]:
        # 垂直于x轴，没有K值
        return None
    else:
        return (p2[1] - p1[1]) * 1.0 / (p2[0] - p1[0])


def get_line_k_and_b(p1, p2):
    """
    湖区某一条直线的斜率和斜距
    :param p1: 点1的坐标
    :param p2: 点2的坐标
    :return:
    """
    if p1[0] == p2[0]:
        # 垂直于x轴,没有k值
        return None, p1[0]
    else:
        k = (p2[1] - p1[1]) / (p2[0] - p1[0])
        b = p1[1] - k * p1[0]
        return k, b


def pix_pos_to_robot(pixPos, matrix):
    """
    一个点的像素坐标,根据标定矩阵,转换为机械手的坐标
    :param pixPos: 图像坐标(x,y)
    :param matrix: 标定矩阵
    :return:
    """
    x, y = pixPos
    robotPos = np.dot((x, y, 1), np.transpose(matrix))
    return round(robotPos[0], 2), round(robotPos[1], 2)


def get_two_points_middle(p1, p2):
    """
    获取两个点的中点坐标
    :param p1: 点1
    :param p2: 点2
    :return:
    """
    assert len(p1) == 2
    assert len(p2) == 2
    middleX = (p1[0] + p2[0]) / 2
    middleY = (p1[1] + p2[1]) / 2
    return round(middleX, 2), round(middleY, 2)


def get_board_length(topLine, midLine, bottomLine):
    """
    根据三条线,以及灯管间距获取板的高度,以及总高度.将x和y互换,换成我们习惯的坐标,以裁切机的原点为原点
    :param topLine:第一条线
    :param midLine: 中线
    :param bottomLine: 第三条线
    :return:
    """
    lineNew = []
    for line in [topLine, midLine, bottomLine]:
        lineTmp = [[line[0][1], line[0][0]], [line[1][1], line[1][0]]]
        lineNew.append(lineTmp)

    topLine, midLine, bottomLine = lineNew
    midLineMidP = get_two_points_middle(midLine[0], midLine[1])
    # 计算板长,用垂直于中线的中点的垂线与topLine的交点,以及和bottomLine的交点来求
    topK, topB = get_line_k_and_b(topLine[0], topLine[1])
    midK, midB = get_line_k_and_b(midLine[0], midLine[1])
    bottomK, bottomB = get_line_k_and_b(bottomLine[0], bottomLine[1])

    if midK == 0:
        midVerTopIntersection = [midLineMidP[0], topK * midLineMidP[0] + topB]
        midVerBottomIntersection = [midLineMidP[0], bottomK * midLineMidP[0] + bottomB]
    else:
        midVerK = -1 / midK
        midVerB = midLineMidP[1] - midVerK * midLineMidP[0]
        midVerTopIntersection = get_two_lines_intersection(midVerK, midVerB, topK, topB)
        midVerBottomIntersection = get_two_lines_intersection(midVerK, midVerB, bottomK, bottomB)
    topToMiddle = distance(midLineMidP, midVerTopIntersection)
    topToBottom = distance(midLineMidP, midVerBottomIntersection) + topToMiddle

    return round(topToMiddle, 2), round(topToBottom, 2)


def get_two_line_angle(p1, p2, p3, p4):
    line1 = Line(p1[1], p1[0], p2[1], p2[0])
    line2 = Line(p3[1], p3[0], p4[1], p4[0])
    v1 = line1.vector()
    v2 = line2.vector()
    len1 = line1.length()
    len2 = line2.length()

    # 获去方向
    derection = v1[0] * v2[1] - v1[1] * v2[0]  # 可以判断向量b到向量a的方向,如果是正的,表示向量b在向量a的顺时针方向.由b到a需要逆时针调整.因为我们规定需要逆时针调整的为正值
    # 获取内积
    inner_mul = np.dot(v1, v2)
    # 获取模积
    model_mul = len1 * len2
    # 向量夹角公式
    cosx = inner_mul / model_mul
    # 防止计算误差,两条线平行的时候值超过1
    cosx = 1 if cosx > 1.0 else cosx
    top_middle_angle = np.arccos(cosx) * (180 / np.pi)
    top_middle_angle = top_middle_angle if derection > 0 else -top_middle_angle
    return round(top_middle_angle, 2)


def get_middle_cut_line(upLeftBottom, upRightBottom, downLeftTop, downRightTop):
    upK, upB = get_line_k_and_b(upLeftBottom, upRightBottom)
    downK, downB = get_line_k_and_b(downLeftTop, downRightTop)
    intersection = get_two_lines_intersection(upK, upB, downK, downB)
    # 如果本身就是平行线没有交点.这样中线的k就是upK 或者downK,都一样,如果确定b的值呢.
    if intersection[0] is None:
        middleLineK = upK
        # 先找到中线上的一个必过点
        middleLineP = np.add(upLeftBottom, downLeftTop) / 2
        middleLineB = middleLineP[1] - middleLineK * middleLineP[0]
    else:
        middleLineK = np.tan((np.arctan(upK) + np.arctan(downK)) / 2)  # 角平分线公式
        middleLineB = intersection[1] - middleLineK * intersection[0]

    return middleLineK, middleLineB


def get_two_lines_intersection(k1, b1, k2, b2):
    """
    获取平面上两条线的交点
    :param k1: 线1的k值
    :param b1: 线1的b值
    :param k2: 线2的k值
    :param b2: 线2的b值
    :return:
    """
    # 如果k相等,说明垂直无交点
    if k1 == k2:
        return None, None
    elif k1 is None and k2 is not None:
        return b1, k2 * b1 + b2
    elif k2 is None and k1 is not None:
        return b2, k1 * b2 + b1
    else:
        x = (b1 - b2) / (k1 - k2) * (-1)
        y = k1 * x + b1
    return x, y


def trans_k_to_rad(k):
    """
    将斜率转换为弧度
    :param k: 斜率k值
    :return:
    """
    return np.arctan(k)


def get_two_line_distance(line1, line2):
    """
    获取两条直线上的距离
    :param line1: 线1
    :param line2: 线2
    :return:
    """
    topLine, bottomLine = line1, line2
    topK, topB = get_line_k_and_b(topLine[0], topLine[1])
    bottomK, bottomB = get_line_k_and_b(bottomLine[0], bottomLine[1])
    bottomMidP = get_two_points_middle(bottomLine[0], bottomLine[1])
    topMiddleP = get_two_points_middle(topLine[0], topLine[1])
    if bottomK == 0:
        if topK == 0:
            lineDis = abs(bottomMidP[1] - topMiddleP[1])
        else:
            bottomMidVerTopIntersection = [bottomMidP[0], topK * bottomMidP[0] + topB]
            lineDis = distance(bottomMidP, bottomMidVerTopIntersection)
    elif bottomK is not None:
        bottomVerK = -1 / bottomK
        bottomVerB = bottomMidP[1] - bottomVerK * bottomMidP[0]
        bottomMidVerTopIntersection = get_two_lines_intersection(topK, topB, bottomVerK, bottomVerB)
        lineDis = distance(bottomMidP, bottomMidVerTopIntersection)
    else:  # 如果两条线竖向垂直的情况
        lineDis = abs(bottomMidP[0] - topMiddleP[0])
    return round(lineDis, 2)


def get_angle_by_k(k):
    """
    根据斜率k值,获取对应的角度值
    :param k: 线条的斜率k值
    :return:
    """
    if k == 0:
        return 0
    if k is None:
        return 90
    return np.arctan(k) * 180 / np.pi


def check_two_box_is_overlap(upBox, downBox):
    """
    判断中缝是否堆叠
    :param imageGray:
    :param upBox:上框信息
    :param downBox: 下框信息
    :return:
    """
    isOverlap = False
    # 首先是判断是否叠板
    for point in upBox:
        if check_point_in_rect(point, downBox):
            isOverlap = True

    for point in downBox:
        if check_point_in_rect(point, upBox):
            isOverlap = True
    return isOverlap


def check_point_in_rect(point, box):
    """
    判断的规则是:
    利用的夹角的正负值,可以确定是顺时针还是逆时针方向.如果在矩形的内部的话,肯定是全部大于90度或者全部小于90度.
    这样就可以利用向量的点积的正负号来判断夹角是大于90度还是小于90度.
    判断一个点是否在矩形内,box是识别出来的一个框的矩形,point是另外一个框的一个点.
    :param point:矩形框的四个顶点的某一个点
    :param box:另外一个矩形框的四个顶点
    :return:
    """
    x, y = point
    Ax, Ay = box[0]
    Bx, By = box[3]
    Cx, Cy = box[2]
    Dx, Dy = box[1]
    a = (Bx - Ax) * (y - Ay) - (By - Ay) * (x - Ax)
    b = (Cx - Bx) * (y - By) - (Cy - By) * (x - Bx)
    c = (Dx - Cx) * (y - Cy) - (Dy - Cy) * (x - Cx)
    d = (Ax - Dx) * (y - Dy) - (Ay - Dy) * (x - Dx)
    if ((a > 0 and b > 0 and c > 0 and d > 0) or (a < 0 and b < 0 and c < 0 and d < 0)):
        return True
    else:
        return False


if __name__ == '__main__':
    line1 = [(0, 0), (30, 0)]
    line2 = [(0, 30), (30, 45)]
    dis = get_two_line_distance(line1, line2)
    print("dis = {}".format(dis))
